#include "RSIM_INC.h"
#include "Cholesky.h"

#include <iostream>

using namespace std;
using namespace RSIM;


int main(){
        // generated using the Matlab command A = gallery('toeppd',6,6,rand(6),rand(6))
        double a[]={    
                 3.490777947126143,  -0.292429867042680,   0.165130347548928,   2.173213024681686,   0.600159187446968,  -0.851956588352890,
                -0.292429867042680,   3.490777947126143,  -0.292429867042680,   0.165130347548928,   2.173213024681686,   0.600159187446968,
                0.165130347548928,  -0.292429867042680,   3.490777947126143,  -0.292429867042680,  0.165130347548928,   2.173213024681686,
                2.173213024681686,   0.165130347548928,  -0.292429867042680,   3.490777947126143,  -0.292429867042680,   0.165130347548928,
                0.600159187446968,   2.173213024681686,   0.165130347548928,  -0.292429867042680,   3.490777947126143,  -0.292429867042680,
                -0.851956588352890,   0.600159187446968,   2.173213024681686,   0.165130347548928,  -0.292429867042680,   3.490777947126143};
        
        Matrix A(6,6,a); 
        Matrix B;
        Mat6x6 C;
        double d[36];

        // A should be symmetric positive definite.
        Cholesky chol(A);        
        
        if(chol.is_spd()){
                // solves for B where, A*B = A. B should be an identity matrix
                chol.solve(A,B);
                
                // Alternatively
                chol.solve(A,C.wPtr());
                
                // Or
                chol.solve(A,d);
                
                cout<<Mat6x6(d)<<endl;                
        }
        return 0;
}